#include <iostream>
#include "nav_dwb_plugins/one_d_velocity_iterator.hpp"

int main() {
    // 定义初始条件
    double current_velocity = -1.1;  // 当前速度 (m/s)
    double min_velocity = -2.0;      // 最小速度 (m/s)
    double max_velocity = 2.0;       // 最大速度 (m/s)
    double acceleration_limit = 1.0; // 加速度限制 (m/s^2)
    double deceleration_limit = 1.0; // 减速度限制 (m/s^2)
    double acceleration_time = 1.0;  // 加速时间 (s)
    int num_samples = 5;             // 样本数量

    // 创建 OneDVelocityIterator 实例
    nav_dwb_plugins::OneDVelocityIterator velocity_iterator(
        current_velocity, min_velocity, max_velocity,
        acceleration_limit, deceleration_limit, acceleration_time, num_samples);

    // 遍历所有速度值
    std::cout << "Generated Velocities:" << std::endl;
    while (!velocity_iterator.isFinished()) {
        std::cout << velocity_iterator.getVelocity() << " m/s" << std::endl;
        ++velocity_iterator;  // 使用 ++(*th_it_) 的形式
    }

    return 0;
}